Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Implementing a basic controller

Introduction

In this tutorial, we will use a KDL Manager class to facilitate the implementation of a simple kinematic robot controller. We will assume you have completed the previous tutorial on initializing a KDL manager class.

Simulating a Robot

First, we need to simulate a robot for us to control. We will use the Robot Kinematic Simulation. Clone the simulator into your catkin workspace

$ cd catkin_ws/src
$ git clone https://github.com/diogoalmeida/robot_kinematic_simulation.git

The simulation works with any robot described in the URDF format. For this tutorial, we will use the Baxter URDF which we got in the previous tutorial. The simulation will publish the robot state at a given rate, and it can be controlled by publishing a JointState message with the desired control velocities for the robot.

The Code

In the src folder of your tutorial package, create a 'simple_control.cpp' file and paste the following code.

https://raw.githubusercontent.com/diogoalmeida/generic_control_toolbox_tutorials/master/kdl_manager_tutorial/src/simple_control.cpp

   1 #include <ros/ros.h>
   2 #include <generic_control_toolbox/kdl_manager.hpp>
   3 
   4 sensor_msgs::JointState state;
   5 
   6 void stateCb(const sensor_msgs::JointState::ConstPtr &msg)
   7 {
   8   state = *msg;
   9 }
  10 
  11 int main (int argc, char ** argv)
  12 {
  13   ros::init(argc, argv, "simple_control");
  14   ros::NodeHandle nh;
  15 
  16   // Define the communications with the robot
  17   ros::Subscriber state_sub = nh.subscribe("/joint_states", 1, &stateCb);
  18   ros::Publisher command_pub = nh.advertise<sensor_msgs::JointState>("/joint_command", 1);
  19 
  20   // Initialize a KDL manager on the robot's left arm
  21   generic_control_toolbox::KDLManager manager("torso");
  22   manager.initializeArm("left_hand");
  23 
  24   KDL::Frame pose;
  25   KDL::Vector desired_position, error;
  26   KDL::JntArray q_dot(7);
  27   KDL::Twist command_vel = KDL::Twist::Zero();
  28   sensor_msgs::JointState command;
  29 
  30   desired_position.x(0.7);
  31   desired_position.y(0.2);
  32   desired_position.z(0.32);
  33 
  34   while (ros::ok())
  35   {
  36     // Get the end-effector pose as a KDL::Frame.
  37     if (manager.getEefPose("left_hand", state, pose))
  38     {
  39       error = desired_position - pose.p;
  40       command_vel.vel = 0.5*error;
  41       ROS_INFO_THROTTLE(0.5,
  42                         "Position error: (%.2f, %.2f, %.2f)",
  43                         error.x(), error.y(), error.z());
  44 
  45       // Get the IK solution for the desired control command.
  46       manager.getVelIK("left_hand", state, command_vel, q_dot);
  47       command = state;
  48       manager.getJointState("left_hand", q_dot.data, command);
  49       command_pub.publish(command);
  50     }
  51 
  52     ros::spinOnce();
  53     ros::Duration(0.1).sleep();
  54   }
  55 
  56   return 0;
  57 }

The Code Explained

Lets break down the code:

  17   ros::Subscriber state_sub = nh.subscribe("/joint_states", 1, &stateCb);
  18   ros::Publisher command_pub = nh.advertise<sensor_msgs::JointState>("/joint_command", 1);

Here, we initialize the ROS publisher and subscriber which allow us to interface with the simulation. The simulator expects a JointState message to deliver the controller output and, in turn, publishes a state message with the current robot state. We will use the current state message to compute the robot forward kinematics and the control error.

  24   KDL::Frame pose;
  25   KDL::Vector desired_position, error;
  26   KDL::JntArray q_dot(7);
  27   KDL::Twist command_vel = KDL::Twist::Zero();
  28   sensor_msgs::JointState command;
  29 
  30   desired_position.x(0.7);
  31   desired_position.y(0.2);
  32   desired_position.z(0.32);

We define the required variables to compute the position error (pose, desired_position and error), and the control output (command_vel, q_dot, command). We set a desired position for the left arm's end-effector (which we defined as the 'left_hand' link of Baxter's left arm).

  37     if (manager.getEefPose("left_hand", state, pose))
  38     {
  39       error = desired_position - pose.p;
  40       command_vel.vel = 0.5*error;
  41       ROS_INFO_THROTTLE(0.5,
  42                         "Position error: (%.2f, %.2f, %.2f)",
  43                         error.x(), error.y(), error.z());
  44 
  45       // Get the IK solution for the desired control command.
  46       manager.getVelIK("left_hand", state, command_vel, q_dot);
  47       command = state;
  48       manager.getJointState("left_hand", q_dot.data, command);
  49       command_pub.publish(command);
  50     }

Here, we compute the desired end-effector velocity, obtain the Inverse Kinematics solution and send the control command to the robot simulator.

  39       error = desired_position - pose.p;
  40       command_vel.vel = 0.5*error;

Here, we compute the desired cartesian command as

latex error! exitcode was 1 (signal 0), transscript follows:

This is pdfTeX, Version 3.14159265-2.6-1.40.19 (TeX Live 2018) (preloaded format=latex)

kpathsea: Running mktexfmt latex.fmt
mktexfmt: mktexfmt is using the following fmtutil.cnf files (in precedence order):
mktexfmt:   /usr/share/texlive/texmf-dist/web2c/fmtutil.cnf
mktexfmt: mktexfmt is using the following fmtutil.cnf file for writing changes:
mktexfmt:   /usr/share/httpd/.texlive2018/texmf-config/web2c/fmtutil.cnf
/usr/bin/mktexfmt: mkdir(/usr/share/httpd/.texlive2018/) failed: Permission denied
I can't find the format file `latex.fmt'!

  46       manager.getVelIK("left_hand", state, command_vel, q_dot);
  47       command = state;
  48       manager.getJointState("left_hand", q_dot.data, command);
  49       command_pub.publish(command);

Finally, the IK solution is computed with the KDL manager.

Building the node

To build your code, add the following to the CMakeLists file of the previous tutorial

  26 add_executable(simple_control src/simple_control.cpp)
  27 target_link_libraries(simple_control ${catkin_LIBRARIES})
  28 add_dependencies(simple_control ${catkin_EXPORTED_TARGETS})

You should now be able to compile the node.

Running your code

To test the algorithm, we will need to launch the node together with the kinematic simulation. Create a launch file 'simple_controller_tutorial.launch' and copy the following in it

  <launch>
  <param name="robot_description" command="$(find xacro)/xacro.py --inorder $(find baxter_description)/urdf/baxter.urdf.xacro gazebo:=false"/>
  <node pkg="robot_state_publisher" type="robot_state_publisher" name="rob_st_pub" />

  <node name="robot_kinematic_simulation" type="kinematic_simulation_node" pkg="robot_kinematic_simulation" output="screen">
    <rosparam command="load" file="$(find kdl_manager_tutorial)/config/simulation.yaml"/>
  </node>
  <node pkg="kdl_manager_tutorial" type="simple_control" name="simple_control" output="screen"/>
</launch>

This will load the robot URDF into the ROS parameter server, start the kinematic simulation, the control load and a robot state publisher. The latter is not required, but allows you to see the robot model in RViz. To launch the controller type

  $ roslaunch kdl_manager_tutorial simple_controller_tutorial.launch

You will see the progression of the control error being printed on the terminal.

To build a more advanced controller which starts and stops at a user's request, follow through to the [[|Controller template tutorial]].